﻿using Accord;
using System;

namespace AMEE.MashineLearning
{
    /// <summary>
    /// Under construction.
    /// </summary>
    /// <typeparam name="TModel">The type of the model.</typeparam>
    /// <typeparam name="TModelData">The type of the model data.</typeparam>
    internal class FittingFunction<TModel, TModelData> where TModel : class
    {
        /// <summary>
        /// Gets or sets the fitting.
        /// </summary>
        /// <value>
        /// The fitting.
        /// </value>
        public Func</*TModelData[]*/int[], TModel> Fitting { get; set; }

        /// <summary>
        /// Gets or sets the fitting2.
        /// </summary>
        /// <value>
        /// The fitting2.
        /// </value>
        public Func<TModelData[], TModel> Fitting2 { get; set; }

        /// <summary>
        /// Gets or sets the minimum sample count.
        /// </summary>
        /// <value>
        /// The minimum sample count.
        /// </value>
        public int MinSampleCount { get; set; }
    }


    /// <summary>
    ///   Multipurpose RANSAC algorithm.
    /// </summary>
    /// 
    /// <typeparam name="TModel">The model type to be trained by RANSAC.</typeparam>
    /// 
    /// <remarks>
    /// <para>
    ///   RANSAC is an abbreviation for "RANdom SAmple Consensus". It is an iterative
    ///   method to estimate parameters of a mathematical model from a set of observed
    ///   data which contains outliers. It is a non-deterministic algorithm in the sense
    ///   that it produces a reasonable result only with a certain probability, with this
    ///   probability increasing as more iterations are allowed.</para>
    ///   
    /// <para>
    ///   References:
    ///   <list type="bullet">
    ///     <item><description>
    ///       P. D. Kovesi. MATLAB and Octave Functions for Computer Vision and Image Processing.
    ///       School of Computer Science and Software Engineering, The University of Western Australia.
    ///       Available in: http://www.csse.uwa.edu.au/~pk/research/matlabfns </description></item>
    ///     <item><description>
    ///       Wikipedia, The Free Encyclopedia. RANSAC. Available on:
    ///       http://en.wikipedia.org/wiki/RANSAC </description></item>
    ///   </list>
    /// </para>
    /// </remarks>
    /// 
    public class RANSAC<TModel/*, TModelData*/> where TModel : class
    {
        // RANSAC parameters
        /// <summary>Number of samples.</summary>
        private int _sMin;
        /// <summary>inlier threshold</summary>
        private double _threshold;
        /// <summary>Default: 100</summary>
        private int _maxSamplings = 100;
        /// <summary>Default: 1000</summary>
        private int _maxEvaluations = 1000;
        /// <summary>Default: 0.99</summary>
        private double _probability = 0.99;

        // RANSAC functions
        //private Func<int[], TModel> _fitting;
        //private Func<TModel, double, int[]> distances;
        //private Func<int[], bool> degenerate;

        Random _random = new Random();

        #region Properties
        /// <summary>
        /// Model fitting function, that creates the model corresponding to specified sample data.
        /// <para>
        /// A function that takes indexes of sample points and returns the model. Eg., takes two
        /// point indexes and returns the line, defined by this two Points
        /// </para>
        /// </summary>
        /// <value></value>
        public Func</*TModelData[]*/int[], TModel> Fitting {
            get;
            set;
        }

        /// <summary>
        /// Degenerative set detection function. (Optional)
        /// <para>The function takes indexes of samples and returns 
        /// if the sampled points are not in a degenerate configuration.</para>
        /// </summary>
        /// <value>
        /// bool degenerateDelegate(int[] indexes)
        /// </value>
        public Func<int[], bool> Degenerate {
            get;
            set;
        }

        /// <summary>
        /// Gets or sets the Distance function.
        /// <para>
        /// Evaluates the distances between total points and the model returning the
        /// </para>
        /// </summary>
        /// <value>
        /// A Function delegate, that takes the model and the threshold and that returns
        /// the indices of the points that are inliers
        /// </value>
        public Func<TModel, double, int[]> Distances {
            get;
            set;
        }

        /// <summary>
        /// Gets or sets the minimum distance between a data point and
        /// the model used to decide whether the point is an inlier or not.
        /// </summary>
        /// <value></value>
        public double Threshold {
            get { return _threshold; }
            set { _threshold = Math.Max(value,0.0); }
        }

        /// <summary>
        /// Gets or sets the minimum number of samples from the data
        /// required by the fitting function to fit a model.
        /// <para>For example two, to define a Line</para>
        /// </summary>
        /// <value></value>
        public int Samples {
            get { return _sMin; }
            set { _sMin = value; }
        }

        /// <summary>
        ///   Maximum number of attempts to select a 
        ///   non-degenerate data set. Default is 100.
        /// </summary>
        /// <value></value>
        /// <remarks>
        ///   The default value is 100.
        /// </remarks>
        public int MaxSamplings {
            get { return _maxSamplings; }
            set { _maxSamplings = value; }
        }

        /// <summary>
        /// Maximum number of iterations. Default is 1000.
        /// </summary>
        /// <remarks>
        ///   The default value is 1000.
        /// </remarks>
        /// <value></value>
        public int MaxEvaluations {
            get { return _maxEvaluations; }
            set { _maxEvaluations = value; }
        }

        /// <summary>
        ///   Gets or sets the probability of obtaining a random
        ///   sample of the input points that contains no outliers.
        ///   Default is 0.99.
        ///   <para>
        ///   The probability is a minimum probability of finnding at least one good 
        ///   set of observations in N trials. It lies usually between 0.90 and 0.99.
        ///   </para>
        /// </summary>
        /// <value>the probability (will be cutted to 0 respectively 1, if value is out of range)</value>
        public double Probability {
            get { return _probability; }
            set { _probability = Math.Min(Math.Max(value,0.0),1.0); }
        }
        #endregion


        /// <summary>
        ///   Constructs a new RANSAC algorithm.
        /// </summary>
        /// 
        /// <param name="minSamples">
        ///   The minimum number of samples from the data
        ///   required by the fitting function to fit a model.
        /// </param>
        /// 
        public RANSAC(int minSamples) {
            Samples = minSamples;
        }

        /// <summary>
        ///   Constructs a new RANSAC algorithm.
        /// </summary>
        /// 
        /// <param name="minSamples">
        ///   The minimum number of samples from the data
        ///   required by the fitting function to fit a model.
        /// </param>
        /// <param name="threshold">
        ///   The minimum distance between a data point and
        ///   the model used to decide whether the point is
        ///   an inlier or not.
        /// </param>
        /// 
        public RANSAC(int minSamples, double threshold) {
            Samples = minSamples;
            Threshold = threshold;
        }

        /// <summary>
        ///   Constructs a new RANSAC algorithm.
        /// </summary>
        /// 
        /// <param name="minSamples">
        ///   The minimum number of samples from the data
        ///   required by the fitting function to fit a model.
        /// </param>
        /// <param name="threshold">
        ///   The minimum distance between a data point and
        ///   the model used to decide whether the point is
        ///   an inlier or not.
        /// </param>
        /// <param name="probability">
        ///   The probability of obtaining a random sample of
        ///   the input points that contains no outliers.
        /// </param>
        /// 
        public RANSAC(int minSamples, double threshold, double probability) {
            if (minSamples < 0)
                throw new ArgumentOutOfRangeException("minSamples");

            if (threshold < 0)
                throw new ArgumentOutOfRangeException("threshold");

            if (probability > 1.0 || probability < 0.0)
                throw new ArgumentException("Probability should be a value between 0 and 1", "probability");

            Samples = minSamples;
            Threshold = threshold;
            Probability = probability;
        }


        /// <summary>
        /// Computes the model using the RANSAC algorithm.
        /// </summary>
        /// <param name="size">The total number of points in the data set.</param>
        /// <returns>The model using the RANSAC algorithm.</returns>
        public TModel Compute(int size) {
            int[] inliers;
            return Compute(size, out inliers);
        }

        /// <summary>
        /// Computes the model using the RANSAC algorithm.
        /// </summary>
        /// <param name="size">The total number of points in the data set.</param>
        /// <param name="inliers">The indexes of the inlier points in the data set.</param>
        /// <returns>The model using the RANSAC algorithm.</returns>
        public TModel Compute(int size, out int[] inliers) {
            // We are going to find the best model (which fits
            //  the maximum number of inlier points as possible).
            TModel bestModel = null;
            int[] bestInliers = null;
            int maxInliers = 0;
            Random random = new Random();

            int r = Math.Min(size, Samples); // Anzahl fürs Modell benötigter Punkte

            // For this we are going to search for random samples
            //  of the original points which contains no outliers.

            int count = 0;  // Total number of trials performed
            double N = MaxEvaluations;   // Estimative of number of trials needed.

            // While the number of trials is less than our estimative,
            //   and we have not surpassed the maximum number of trials
            while (count < N && count < MaxEvaluations) {
                TModel model = null;
                int[] sample = null;
                int samplings = 0;

                // Give 'MaxSampling' tries to find a non-degenerated plane sample
                while (samplings < MaxSamplings) {
                    // Select at random 'size' data points to form a trial model.
                    // !! -- Geänderte Methode, weil ursprüngliche bei 200k Punkten zu langsam -- !!
                    sample = RandomSample(size, r); // gib mir r(=MinSamples) Indizes von 0 bis size(=Gesamtzahl Samples)

                    // If the sampled points are not in a degenerate configuration,
                    if (Degenerate == null || !Degenerate(sample)) {
                        // Fit model using the random selection of points
                        model = Fitting(sample);
                        break; // Exit the while loop.
                    }

                    samplings++; // Increase the samplings counter
                }

                if (model == null)
                    throw new ConvergenceException("A model could not be inferred from the data points");

                // Now, evaluate the distances between total points and the model returning the
                //  indices of the points that are inliers (according to a distance threshold t).
                inliers = Distances(model, Threshold);

                // Check if the model was the model which highest number of inliers:
                if (bestInliers == null || inliers.Length > maxInliers) {
                    // Yes, this model has the highest number of inliers.

                    maxInliers = inliers.Length;  // Set the new maximum,
                    bestModel = model;            // This is the best model found so far,
                    bestInliers = inliers;        // Store the indices of the current inliers.

                    // Update estimate of N, the number of trials to ensure we pick, 
                    //   with probability p, a data set with no outliers.
                    double pInlier = (double)inliers.Length / (double)size;
                    double pNoOutliers = 1.0 - System.Math.Pow(pInlier, Samples);

                    N = System.Math.Log(1.0 - Probability) / System.Math.Log(pNoOutliers);
                }

                count++; // Increase the trial counter.
            }

            inliers = bestInliers;
            return bestModel;
        }

        /// <summary>
        /// Get <c>count</c> random indexes from 0 (incl.) to <c>max</c> (excl).
        /// <para>
        /// Modification by B.Bettzueche.
        /// </para>
        /// </summary>
        /// <param name="max">The exclusive maximum value (greater 0)</param>
        /// <param name="count">Number of Indexes to be generated (greater 0).</param>
        /// <returns>Array of size <c>count</c> and of random indexes up to <c>max</c> exclusive</returns>
        int[] RandomSample(int max, int count) {
            int[] resu = new int[count];
            for (int i = 0; i < count; i++) {
                resu[i] = _random.Next(max);
            }
            return resu;
        } 

    }
}
